import os
import time
import sys
import logging
from direction_enum import Direction

# PWM 配置
PWM_CHIP1 = "pwmchip0"  # 第一个舵机使用pwmchip0
PWM_CHIP2 = "pwmchip1"  # 第二个舵机使用pwmchip1
PWM_CHANNEL1 = "0"      # 两个芯片都使用通道0
PWM_CHANNEL2 = "0"
PWM_PATH1 = f"/sys/class/pwm/{PWM_CHIP1}/pwm{PWM_CHANNEL1}"
PWM_PATH2 = f"/sys/class/pwm/{PWM_CHIP2}/pwm{PWM_CHANNEL2}"

# 舵机参数（MG90S 180°）
PWM_FREQ = 50          # 50Hz，周期20ms
PERIOD_NS = 20000000   # 20ms in nanoseconds (去掉下划线)
MIN_PULSE = 500000     # 0.5ms 对应0°
MAX_PULSE = 2500000    # 2.5ms 对应180°
STOP_PULSE = 1500000   # 1.5ms 对应90°（停止位置）

# 双目标角度
TARGET_ANGLE1 = 180    # 舵机1默认目标角度
TARGET_ANGLE2 = 90     # 舵机2默认目标角度

# 当前角度（全局变量）
current_angle1 = 180  # 舵机1初始位置
current_angle2 = 0  # 舵机2初始位置

log_dir = os.path.expanduser("~/project_code2")
os.makedirs(log_dir, exist_ok=True)
log_file = os.path.join(log_dir, "app.log")

logging.basicConfig(
    level=logging.DEBUG,
    format="%(asctime)s [%(levelname)s] %(message)s",
    handlers=[logging.FileHandler(log_file), logging.StreamHandler()]
)

def setup_pwm():
    """初始化双PWM通道"""
    # 初始化PWM通道1
    if not os.path.exists(f"/sys/class/pwm/{PWM_CHIP1}"):
        logging.error(f"[servo] PWM设备{PWM_CHIP1}不存在")
        sys.exit(1)
    init_pwm_channel(PWM_CHIP1, PWM_CHANNEL1, PWM_PATH1)
    
    # 初始化PWM通道2
    if not os.path.exists(f"/sys/class/pwm/{PWM_CHIP2}"):
        logging.error(f"[servo] PWM设备{PWM_CHIP2}不存在")
        sys.exit(1)
    init_pwm_channel(PWM_CHIP2, PWM_CHANNEL2, PWM_PATH2)

def init_pwm_channel(chip, channel, path):
    """初始化单个PWM通道"""
    # 清理旧的PWM通道
    if os.path.exists(path):
        try:
            with open(f"/sys/class/pwm/{chip}/unexport", "w") as f:
                f.write(channel)
            time.sleep(0.1)
        except Exception as e:
            logging.warning(f"[servo] 清理旧PWM通道{channel}失败 - {e}")

    # 导出PWM通道
    try:
        with open(f"/sys/class/pwm/{chip}/export", "w") as f:
            f.write(channel)
        time.sleep(0.5)  # 增加延迟确保设备创建完成
    except PermissionError:
        logging.error(f"[servo] 无法导出PWM通道{channel}，请使用sudo运行")
        sys.exit(1)
    except Exception as e:
        logging.error(f"[servo] 导出PWM通道{channel}失败 - {e}")
        sys.exit(1)

    # 检查设备是否创建成功
    if not os.path.exists(path):
        logging.error(f"[servo] PWM设备{path}创建失败")
        sys.exit(1)

    # 设置PWM参数
    try:
        with open(f"{path}/period", "w") as f:
            f.write(str(PERIOD_NS))
        with open(f"{path}/polarity", "w") as f:
            f.write("normal")
        with open(f"{path}/duty_cycle", "w") as f:
            f.write(str(STOP_PULSE))
        with open(f"{path}/enable", "w") as f:
            f.write("1")
        logging.info(f"[servo] 舵机PWM通道{chip}:{channel}初始化完成")
    except PermissionError:
        logging.error(f"[servo] 设置PWM通道{channel}参数失败，请使用sudo运行")
        sys.exit(1)
    except Exception as e:
        logging.error(f"[servo] 设置PWM参数失败 - {e}")
        sys.exit(1)

def set_pulse(pulse_ns, channel=1):
    """设置指定通道的PWM脉宽"""
    path = PWM_PATH1 if channel == 1 else PWM_PATH2
    try:
        with open(f"{path}/duty_cycle", "w") as f:
            f.write(str(pulse_ns))
        logging.debug(f"[servo] 通道{channel}设置脉宽: {pulse_ns/1000000}ms")
    except PermissionError:
        #logging.error(f"[servo] 无法设置通道{channel}脉宽，请使用sudo运行")
        sys.exit(1)

def set_servo_angle(target_angle, servo_id=1):
    """控制指定舵机旋转到目标角度（0°到180°）"""
    global current_angle1, current_angle2

    if not 0 <= target_angle <= 180:
        raise ValueError(f"[servo] 角度必须在0到180度之间（收到: {target_angle}）")

    if servo_id == 1:
        current_angle = current_angle1
    elif servo_id == 2:
        current_angle = current_angle2
    else:
        raise ValueError("[servo] 舵机ID必须是1或2")

    pulse_width = int(MIN_PULSE + (MAX_PULSE - MIN_PULSE) * (target_angle / 180.0))
    angle_diff = abs(target_angle - current_angle)
    rotate_time = angle_diff * 0.1 / 60  # 每60度需要0.1秒

    #logging.info(f"[servo] 舵机{servo_id}从 {current_angle}° 旋转到 {target_angle}°，脉宽: {pulse_width}ns，预计时间: {rotate_time:.2f}s")

    set_pulse(pulse_width, servo_id)
    time.sleep(rotate_time)
    
    if servo_id == 1:
        current_angle1 = target_angle
    else:
        current_angle2 = target_angle
        
    #logging.info(f"[servo] 舵机{servo_id}到达目标角度: {target_angle}°")

def cleanup_pwm():
    """关闭双PWM通道"""
    cleanup_channel(PWM_CHIP1, PWM_CHANNEL1, PWM_PATH1)
    cleanup_channel(PWM_CHIP2, PWM_CHANNEL2, PWM_PATH2)

def cleanup_channel(chip, channel, path):
    """清理单个PWM通道"""
    if os.path.exists(path):
        try:
            with open(f"{path}/enable", "w") as f:
                f.write("0")
            with open(f"/sys/class/pwm/{chip}/unexport", "w") as f:
                f.write(channel)
            logging.info(f"[servo] PWM通道{chip}:{channel}已清理")
        except Exception as e:
            logging.warning(f"[servo] 清理PWM通道{channel}失败 - {e}")


def direction(direction:int):
    if direction==0:
        set_servo_angle(180, 1)  
        set_servo_angle(0, 2)  
    if direction==1:
        set_servo_angle(90, 1)  
        set_servo_angle(180, 2)  
    if direction==2:
        set_servo_angle(0, 1)  
        set_servo_angle(0, 2)  
    if direction==3:
        set_servo_angle(90, 1)  
        set_servo_angle(0, 2)  



if __name__ == "__main__":
    try:
        setup_pwm()
        direction(1)
        time.sleep(1)
        direction(2)
        time.sleep(1)
        direction(3)
        time.sleep(1)
        direction(4)
         

        # 测试两个舵机
        
        
    except Exception as e:
        logging.error(f"[servo] 错误: {e}")
    finally:
        cleanup_pwm()